﻿using System;
using com.atiia.automation.sensors;
using System.Windows.Forms;

namespace ControlSystem
{
    /**
     *
     * @author Johnathan Pretorius
     * Transcoded by Albie Van Zyl
     */

    public class HapticControl
    {
        FT ftf;
        FT add;
        FT ft1;
        FT ft2;
        static int MAX_ExecutionTime = 250; //units: ms
        static int MIN_ExecutionTime = 250; //units: ms

        static double MIN_distance = 0.5;
        static double MAX_distance = 10;

        static double MAXSPEED = 50; //units: mm/s
    
        static double MIN_angle = 0.5;
        static double MAX_angle = 5;

        static double Mass = 0.5; //units: kg was 3
        static double penRadius = 0.005; //units: meters
        static double penLenght = 0.08; //units: meters
        static double[] Ixyz = {(Mass*Math.Pow(penRadius,2)/4 + Mass*Math.Pow(penLenght,2)/3)*10,
                                  (Mass*Math.Pow(penRadius,2)/4 + Mass*Math.Pow(penLenght,2)/3)*10,
                                  (Mass*Math.Pow(penRadius,2)/4 + Mass*Math.Pow(penLenght,2)/3)*10};

        double tyd = 0.03; // units: seconds was 0.1
        double Vtot = 5;    // was 10
        double Vatot = 5;
        double Vrtot = 0;
        double Atot = 0;
        double Aatot = 0;
        double Ftot = 0;
        double Ttot = 0;
        double plusTime = 0;
        int Vtype = 0;
    
        private bool sensor_valid = false;

        public bool IsSensorValid()
        {
            return sensor_valid;
        }

        public HapticControl()
        {
            ftf = new FT();
            add = new FT();
            ft1 = new FT();
            ft2 = new FT();
        }

        public void UpdateSensor1(NetFTRDTPacket sensorpacket)
        {
            ft1 = SensorData2Control(sensorpacket);
            if (Math.Abs(ft1.time - ft2.time) < 250)
                sensor_valid = true;
            else
                sensor_valid = false;
        }

        public void UpdateSensor2(NetFTRDTPacket sensorpacket)
        {
            ft2 = SensorData2Control(sensorpacket);
            if (Math.Abs(ft1.time - ft2.time) < 250)
                sensor_valid = true;
            else
                sensor_valid = false;
        }

        public FT SensorData2Control(NetFTRDTPacket sensorpacket)
        {
            long time = DateTime.Now.Ticks;
            FT getData = new FT();
            getData.Fx = sensorpacket.getFx() / 1e6;
            getData.Fy = sensorpacket.getFy() / 1e6;
            getData.Fz = sensorpacket.getFz() / 1e6;
            getData.Tx = sensorpacket.getTx() / 1e6;
            getData.Ty = sensorpacket.getTy() / 1e6;
            getData.Tz = sensorpacket.getTz() / 1e6;
            getData.time = time;
            return getData;
        }

        public FT CombineSensor(FT ft1, FT ft2)
        {
            //    moet dalk nog verander
            // Fx
            if ((ft1.Fx >= 0 && ft2.Fx >= 0) || (ft1.Fx < 0 && ft2.Fx < 0))
                ftf.Fx = ft1.Fx;
            else
            {
                if (Math.Abs(ft1.Fx) >= Math.Abs(ft2.Fx))
                    ftf.Fx = ft1.Fx + ft2.Fx;
                else ftf.Fx = 0;
            }
            // Fy
            if ((ft1.Fy >= 0 && ft2.Fy >= 0) || (ft1.Fy < 0 && ft2.Fy < 0))
                ftf.Fy = ft1.Fy;
            else
            {
                if (Math.Abs(ft1.Fy) >= Math.Abs(ft2.Fy))
                    ftf.Fy = ft1.Fy + ft2.Fy;
                else ftf.Fy = 0;
            }
            // Fz
            if ((ft1.Fz >= 0 && ft2.Fz >= 0) || (ft1.Fz < 0 && ft2.Fz < 0))
                ftf.Fz = ft1.Fz;
            else
            {
                if (Math.Abs(ft1.Fz) >= Math.Abs(ft2.Fz))
                    ftf.Fz = ft1.Fz + ft2.Fz;
                else ftf.Fz = 0;
            }
            // Tx
            if ((ft1.Tx >= 0 && ft2.Tx >= 0) || (ft1.Tx < 0 && ft2.Tx < 0))
                ftf.Tx = ft1.Tx;
            else
            {
                if (Math.Abs(ft1.Tx) >= Math.Abs(ft2.Tx))
                    ftf.Tx = ft1.Tx + ft2.Tx;
                else ftf.Tx = 0;
            }
            // Ty
            if ((ft1.Ty >= 0 && ft2.Ty >= 0) || (ft1.Ty < 0 && ft2.Ty < 0))
                ftf.Ty = ft1.Ty;
            else
            {
                if (Math.Abs(ft1.Ty) >= Math.Abs(ft2.Ty))
                    ftf.Ty = ft1.Ty + ft2.Ty;
                else ftf.Ty = 0;
            }
            // Tz
            if ((ft1.Tz >= 0 && ft2.Tz >= 0) || (ft1.Tz < 0 && ft2.Tz < 0))
                ftf.Tz = ft1.Tz;
            else
            {
                if (Math.Abs(ft1.Tz) >= Math.Abs(ft2.Tz))
                    ftf.Tz = ft1.Tz + ft2.Tz;
                else ftf.Tz = 0;
            }
            // TESTING

            //END

            return ftf;
        }

        public FT RobotIncValues(FT ft)
        {
            add.Fx = 0.5 * 1000 * (ft.Fx / Mass) * Math.Pow(tyd, 2); //units: mm
            add.Fy = 0.5 * 1000 * ((ft.Fy / Mass) * Math.Pow(tyd, 2)); //units: mm
            add.Fz = 0.5 * 1000 * ((ft.Fz / Mass) * Math.Pow(tyd, 2)); //units: mm

            double TimePI = (Math.Pow(tyd, 2)) * (180 / Math.PI);
            add.Tx = (ft.Tx / Ixyz[0] * TimePI);//units: deg
            add.Ty = (ft.Ty / Ixyz[1] * TimePI);//units: deg
            add.Tz = (ft.Tz / Ixyz[2] * TimePI);//units: deg

            return add;
        }

        public static String RobotCmnd(double Vtot, FT dist)
        {
            String cmnd = "0";
            String Time = String.Format("{0:0}", dist.time);
            String Vtot2 = String.Format("{0:0.00}", Vtot);
            String dFx = String.Format("{0:0.000}", dist.Fx);
            String dFy = String.Format("{0:0.000}", dist.Fy);
            String dFz = String.Format("{0:0.000}", dist.Fz);
            String dTx = String.Format("{0:0.00}", dist.Tx);
            String dTy = String.Format("{0:0.00}", dist.Ty);
            String dTz = String.Format("{0:0.00}", dist.Tz);

            // Only force, const speed
            cmnd = "IMOV " +           //command identifier
                     "0, " +           //Motion speed selection (0 : V (speed), 1 : VR (posture speed))
                     Vtot2 +          //Motion speed
                     ", 26, " +           //Coordinate specification : 26 - Tool coordinate
                     dFx + ", " +   // X coordinate incremental value (unit : mm, significant 3 decimal points)
                     dFy + ", " +   //Y coordinate incremental value (unit : mm, significant 3 decimal points)
                     dFz + ", " +   // Z coordinate incremental value (unit : mm, significant 3 decimal points)
                     dTx + ", " +   //Wrist angle TX incremental value (unit : degree (°), significant 2 decimal points)
                     dTy + ", " +   //Wrist angle TY incremental value (unit : degree (°), significant 2 decimal points)
                     dTz + ", " +   //Wrist angle TZ incremental value (unit : degree (°), significant 2 decimal points)
                     "0, 0, 0, 0, 0, 0, 0, 0";  //not used by spesific robot arm

            // //Only angle, const speed
            //       cmnd =  Time        +
            //               "IMOV "     +           //command identifier
            //                "0, "      +           //Motion speed selection (0 : V (speed), 1 : VR (posture speed))
            //                Vtot2      +          //Motion speed
            //                ", 26, "   +           //Coordinate specification : 26 - Tool coordinate
            //                0 + ", " +   // X coordinate incremental value (unit : mm, significant 3 decimal points)
            //                0 + ", " +   //Y coordinate incremental value (unit : mm, significant 3 decimal points)
            //                0 + ", " +   // Z coordinate incremental value (unit : mm, significant 3 decimal points)
            //                dTx + ", "   +   //Wrist angle TX incremental value (unit : degree (°), significant 2 decimal points)
            //                dTy + ", "   +   //Wrist angle TY incremental value (unit : degree (°), significant 2 decimal points)
            //                dTz + ", "   +   //Wrist angle TZ incremental value (unit : degree (°), significant 2 decimal points)
            //                "0, 0, 0, 0, 0, 0, 0, 0";  //not used by spesific robot arm

            ////Complete - both force and wrist angle
            //        cmnd =  Time        +
            //                "IMOV "     +           //command identifier
            //                "0, "       +           //Motion speed selection (0 : V (speed), 1 : VR (posture speed))
            //                Vtot2        +           //Motion speed
            //                ", 26, "    +           //Coordinate specification : 26 - Tool coordinate
            //                dFx + ", "   +   // X coordinate incremental value (unit : mm, significant 3 decimal points)
            //                dFy + ", "   +   //Y coordinate incremental value (unit : mm, significant 3 decimal points)
            //                dFz + ", "   +   // Z coordinate incremental value (unit : mm, significant 3 decimal points)
            //                dTx + ", "   +   //Wrist angle TX incremental value (unit : degree (°), significant 2 decimal points)
            //                dTy + ", "   +   //Wrist angle TY incremental value (unit : degree (°), significant 2 decimal points)
            //                dTz + ", "   + //Wrist angle TZ incremental value (unit : degree (°), significant 2 decimal points)
            //                "0, 0, 0, 0, 0, 0, 0, 0";  //not used by spesific robot arm

            return cmnd;
        }

        public FT Inc_Control_Robot()
        {
            ft2.CorrectFT2();
            ftf = CombineSensorActive(ft1, ft2);
            ftf = setMaxDistanceTime(ftf);
            ftf = SensorRobotCalibration(ftf);

            Ftot = Math.Sqrt(Math.Pow(ftf.Fx, 2) + Math.Pow(ftf.Fy, 2) + Math.Pow(ftf.Fz, 2));
            Ttot = Math.Sqrt(Math.Pow(ftf.Tx, 2) + Math.Pow(ftf.Ty, 2) + Math.Pow(ftf.Tz, 2));
            Atot = Ftot / Mass;
            Aatot = Ttot / Ixyz[0];
            Vatot = Aatot * tyd * (180 / Math.PI) / 100;
            Vtot = Atot * tyd * 100;
            if (Vtot > Vatot)
            {
                ftf.Vrtot = Vtot;
                ftf.Vtype = 0;
            }
            else
            {
                ftf.Vrtot = Vatot;
                ftf.Vtype = 1;
            }

            if (Vrtot > MAXSPEED)
            {
                ftf.Vrtot = MAXSPEED;
            }

            ftf = RobotIncValues(ftf);
            ftf.time = 0;
            ftf.time = MinSensitivity(ftf);
            //ftf = CombineSensor(ft2, ft1);
            //ftf = RobotIncValues(ftf);
            //ftf = setMaxDistanceTime(ftf);
            //ftf = SensorRobotCalibration(ftf);
            //plusTime = MinSensitivity(ftf);

            return ftf;
        }

        private FT CombineSensorActive(FT ft1, FT ft2)
        {
            FT ftf = new FT();
            ftf.Fx = ft1.Fx + ft2.Fx;
            ftf.Fy = ft1.Fy + ft2.Fy;
            ftf.Fz = ft1.Fz + ft2.Fz;
            ftf.Tx = ft1.Tx + ft2.Tx;
            ftf.Ty = ft1.Ty + ft2.Ty;
            ftf.Tz = ft1.Tz + ft2.Tz;
            return ftf;
        }

        public FT setMaxDistanceTime(FT add)
        {
            ////Force
            if (add.Fx > MAX_distance)
                add.Fx = MAX_distance;
            else
            {
                if (add.Fx < -1 * MAX_distance)
                    add.Fx = -1 * MAX_distance;
            }

            if (add.Fy > MAX_distance)
                add.Fy = MAX_distance;
            else
            {
                if (add.Fy < -1 * MAX_distance)
                    add.Fy = -1 * MAX_distance;
            }

            if (add.Fz > MAX_distance)
                add.Fz = MAX_distance;
            else
            {
                if (add.Fz < -1 * MAX_distance)
                    add.Fz = -1 * MAX_distance;
            }
            /////Torque
            if (add.Tx > MAX_angle)
                add.Tx = MAX_angle;
            else
            {
                if (add.Tx < -1 * MAX_angle)
                    add.Tx = -1 * MAX_angle;
            }

            if (add.Ty > MAX_angle)
                add.Ty = MAX_angle;
            else
            {
                if (add.Ty < -1 * MAX_angle)
                    add.Ty = -1 * MAX_angle;
            }

            if (add.Tz > MAX_angle)
                add.Tz = MAX_angle;
            else
            {
                if (add.Tz < -1 * MAX_angle)
                    add.Tz = -1 * MAX_angle;
            }

            add.time = 1000 * (Math.Sqrt(Math.Pow(add.Fx, 2) +
                                        Math.Pow(add.Fy, 2) +
                                        Math.Pow(add.Fz, 2) +
                                        Math.Pow(add.Tx, 2) +
                                        Math.Pow(add.Ty, 2) +
                                        Math.Pow(add.Tz, 2))) / Vtot;

            if (add.time < MIN_ExecutionTime)
            {
                add.time = MIN_ExecutionTime;
            }

            if (add.time > MAX_ExecutionTime)
            {
                add.time = MAX_ExecutionTime;
            }

            return add;
        }

        public FT SensorRobotCalibration(FT cal)
        {
            cal.Fx = -cal.Fx;
            cal.Fy = -cal.Fy;

            cal.Tx = -cal.Tx;
            cal.Ty = -cal.Ty;

            return cal;
        }

        public void BiasFT(double[] biasData, int id)
        {
            if (id == 1)
            {
                ft1.Fx -= biasData[0];
                ft1.Fy -= biasData[1];
                ft1.Fz -= biasData[2];
                ft1.Tx -= biasData[3];
                ft1.Ty -= biasData[4];
                ft1.Tz -= biasData[5];
            }
            else if (id == 2)
            {
                ft2.Fx -= biasData[0];
                ft2.Fy -= biasData[1];
                ft2.Fz -= biasData[2];
                ft2.Tx -= biasData[3];
                ft2.Ty -= biasData[4];
                ft2.Tz -= biasData[5];
            }
        }

        public double MinSensitivity(FT minS)
        {
            if ((Math.Abs(minS.Fx) < MIN_distance) &&
                 (Math.Abs(minS.Fy) < MIN_distance) &&
                 (Math.Abs(minS.Fz) < MIN_distance) &&
                 (Math.Abs(minS.Ty) < MIN_angle) &&
                 (Math.Abs(minS.Ty) < MIN_angle) &&
                 (Math.Abs(minS.Ty) < MIN_angle))
            {
                minS.time = -1; // indicates that movements are too small
            }

            return minS.time;
        }

        internal void RoundFTs()
        {
            ft1.RoundFT();
            ft2.RoundFT();
        }
    }
}